﻿

#pragma once


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE FFLOAT16::FFLOAT16( float f )
{
    FFloat32To16Array(this, &f, 1);
}
FINLINE FFLOAT16::FFLOAT16( const FFLOAT16& f )
{
    vl = f.vl;
}
FINLINE FFLOAT16::operator float ()
{
    FLOAT f;
    FFloat16To32Array(&f, this, 1);
    return f;
}
FINLINE bool FFLOAT16::operator == ( const FFLOAT16& f ) const
{
    return vl == f.vl;
}
FINLINE bool FFLOAT16::operator != ( const FFLOAT16& f ) const
{
    return vl != f.vl;
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE FVECTOR2::FVECTOR2( const float *pf )
{
    x = pf[0];
    y = pf[1];
}
FINLINE FVECTOR2::FVECTOR2( float fx, float fy )
{
    x = fx;
    y = fy;
}
FINLINE FVECTOR2::operator float* ()
{
    return (float *) &x;
}
FINLINE FVECTOR2::operator const float* () const
{
    return (const float *) &x;
}
FINLINE FVECTOR2& FVECTOR2::operator += ( const FVECTOR2& v )
{
    x += v.x;
    y += v.y;
    return *this;
}
FINLINE FVECTOR2& FVECTOR2::operator -= ( const FVECTOR2& v )
{
    x -= v.x;
    y -= v.y;
    return *this;
}
FINLINE FVECTOR2& FVECTOR2::operator *= ( float f )
{
    x *= f;
    y *= f;
    return *this;
}
FINLINE FVECTOR2& FVECTOR2::operator /= ( float f )
{
    float fInv = 1.0f / f;
    x *= fInv;
    y *= fInv;
    return *this;
}
FINLINE FVECTOR2 FVECTOR2::operator + () const
{
    return *this;
}
FINLINE FVECTOR2 FVECTOR2::operator - () const
{
    return FVECTOR2(-x, -y);
}
FINLINE FVECTOR2 FVECTOR2::operator + ( const FVECTOR2& v ) const
{
    return FVECTOR2(x + v.x, y + v.y);
}
FINLINE FVECTOR2 FVECTOR2::operator - ( const FVECTOR2& v ) const
{
    return FVECTOR2(x - v.x, y - v.y);
}
FINLINE FVECTOR2 FVECTOR2::operator * ( float f ) const
{
    return FVECTOR2(x * f, y * f);
}
FINLINE FVECTOR2 FVECTOR2::operator / ( float f ) const
{
    float fInv = 1.0f / f;
    return FVECTOR2(x * fInv, y * fInv);
}
FINLINE FVECTOR2 operator * ( float f, const FVECTOR2& v )
{
    return FVECTOR2(f * v.x, f * v.y);
}
FINLINE float operator^ (const FVECTOR2 &v, const FVECTOR2 &u)
{
    return v.Angle(&u);
}
FINLINE float operator% (const FVECTOR2 &v, const FVECTOR2 &u)
{
    return FVec2Dot(&v, &u);
}
FINLINE bool FVECTOR2::operator == ( const FVECTOR2& v ) const
{
    return x == v.x && y == v.y;
}
FINLINE bool FVECTOR2::operator != ( const FVECTOR2& v ) const
{
    return x != v.x || y != v.y;
}
FINLINE void FVECTOR2::Set(float nx, float ny)
{
    x = nx;
    y = ny;
}
FINLINE float FVECTOR2::Angle(const FVECTOR2 *v) const
{
    if (v)
    {
        FVECTOR2 s = *this, t = *v;
        s.Normalize();
        t.Normalize();
        return acosf(FVec2Dot(&s, &t));
    }
    else
        return atan2f(y, x);
}
FINLINE void FVECTOR2::Clamp(const float max)
{
    if(FVec2Length(this) > max)
    {
        Normalize();
        x *= max; y *= max;
    }
}
FINLINE FVECTOR2* FVECTOR2::Normalize()
{
    return FVec2Normalize(this, this);
}
FINLINE FVECTOR2* FVECTOR2::Rotate(float a)
{
    FVECTOR2 v;
    v.x = x * cosf(a) - y * sinf(a);
    v.y = x * sinf(a) + y * cosf(a);
    x = v.x;
    y = v.y;
    return this;
}
FINLINE float FVECTOR2::Length()
{
    return FVec2Length(this);
}
FINLINE float FVECTOR2::LengthSq()
{
    return FVec2LengthSq(this);
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE FVECTOR3::FVECTOR3( const float *pf )
{
    x = pf[0];
    y = pf[1];
    z = pf[2];
}
FINLINE FVECTOR3::FVECTOR3( const FVECTOR3& v )
{
    x = v.x;
    y = v.y;
    z = v.z;
}
FINLINE FVECTOR3::FVECTOR3( float fx, float fy, float fz )
{
    x = fx;
    y = fy;
    z = fz;
}
FINLINE FVECTOR3::operator float* ()
{
    return (float *) &x;
}
FINLINE FVECTOR3::operator const float* () const
{
    return (const float *) &x;
}
FINLINE FVECTOR3& FVECTOR3::operator += ( const FVECTOR3& v )
{
    x += v.x;
    y += v.y;
    z += v.z;
    return *this;
}
FINLINE FVECTOR3& FVECTOR3::operator -= ( const FVECTOR3& v )
{
    x -= v.x;
    y -= v.y;
    z -= v.z;
    return *this;
}
FINLINE FVECTOR3& FVECTOR3::operator += ( float v )
{
    x += v;
    y += v;
    z += v;
    return *this;
}
FINLINE FVECTOR3& FVECTOR3::operator -= ( float v )
{
    x -= v;
    y -= v;
    z -= v;
    return *this;
}
FINLINE FVECTOR3& FVECTOR3::operator *= ( float f )
{
    x *= f;
    y *= f;
    z *= f;
    return *this;
}
FINLINE FVECTOR3& FVECTOR3::operator /= ( float f )
{
    float fInv = 1.0f / f;
    x *= fInv;
    y *= fInv;
    z *= fInv;
    return *this;
}
FINLINE FVECTOR3 FVECTOR3::operator + () const
{
    return *this;
}
FINLINE FVECTOR3 FVECTOR3::operator - () const
{
    return FVECTOR3(-x, -y, -z);
}
FINLINE FVECTOR3 FVECTOR3::operator + ( const FVECTOR3& v ) const
{
    return FVECTOR3(x + v.x, y + v.y, z + v.z);
}
FINLINE FVECTOR3 FVECTOR3::operator - ( const FVECTOR3& v ) const
{
    return FVECTOR3(x - v.x, y - v.y, z - v.z);
}
FINLINE FVECTOR3 FVECTOR3::operator * ( float f ) const
{
    return FVECTOR3(x * f, y * f, z * f);
}
FINLINE FVECTOR3 FVECTOR3::operator / ( float f ) const
{
    float fInv = 1.0f / f;
    return FVECTOR3(x * fInv, y * fInv, z * fInv);
}
FINLINE FVECTOR3 operator * ( float f, const struct FVECTOR3& v )
{
    return FVECTOR3(f * v.x, f * v.y, f * v.z);
}
FINLINE bool FVECTOR3::operator == ( const FVECTOR3& v ) const
{
    return x == v.x && y == v.y && z == v.z;
}
FINLINE bool FVECTOR3::operator != ( const FVECTOR3& v ) const
{
    return x != v.x || y != v.y || z != v.z;
}
FINLINE void FVECTOR3::Set( float fx, float fy, float fz )
{
    x = fx;
    y = fy;
    z = fz;
}
FINLINE void FVECTOR3::Set(DWORD vl)
{
    x = FGetRF(vl);
    y = FGetGF(vl);
    z = FGetBF(vl);
}
FINLINE float FVECTOR3::Length() const
{
    return FVec3Length(this);
}
FINLINE float FVECTOR3::LengthSq() const
{
    return FVec3LengthSq(this);
}
FINLINE FVECTOR3* FVECTOR3::Normalize()
{
    return FVec3Normalize(this, this);
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE FVECTOR4::FVECTOR4( const float *pf )
{
    x = pf[0];
    y = pf[1];
    z = pf[2];
    w = pf[3];
}
FINLINE FVECTOR4::FVECTOR4( const FVECTOR3& v, float f )
{
    x = v.x;
    y = v.y;
    z = v.z;
    w = f;
}
FINLINE FVECTOR4::FVECTOR4( float fx, float fy, float fz, float fw )
{
    x = fx;
    y = fy;
    z = fz;
    w = fw;
}
FINLINE FVECTOR4::operator float* ()
{
    return (float *) &x;
}
FINLINE FVECTOR4::operator const float* () const
{
    return (const float *) &x;
}
FINLINE FVECTOR4& FVECTOR4::operator += ( const FVECTOR4& v )
{
    x += v.x;
    y += v.y;
    z += v.z;
    w += v.w;
    return *this;
}
FINLINE FVECTOR4& FVECTOR4::operator -= ( const FVECTOR4& v )
{
    x -= v.x;
    y -= v.y;
    z -= v.z;
    w -= v.w;
    return *this;
}
FINLINE FVECTOR4& FVECTOR4::operator *= ( float f )
{
    x *= f;
    y *= f;
    z *= f;
    w *= f;
    return *this;
}
FINLINE FVECTOR4& FVECTOR4::operator /= ( float f )
{
    float fInv = 1.0f / f;
    x *= fInv;
    y *= fInv;
    z *= fInv;
    w *= fInv;
    return *this;
}
FINLINE FVECTOR4 FVECTOR4::operator + () const
{
    return *this;
}
FINLINE FVECTOR4 FVECTOR4::operator - () const
{
    return FVECTOR4(-x, -y, -z, -w);
}
FINLINE FVECTOR4 FVECTOR4::operator + ( const FVECTOR4& v ) const
{
    return FVECTOR4(x + v.x, y + v.y, z + v.z, w + v.w);
}
FINLINE FVECTOR4 FVECTOR4::operator - ( const FVECTOR4& v ) const
{
    return FVECTOR4(x - v.x, y - v.y, z - v.z, w - v.w);
}
FINLINE FVECTOR4 FVECTOR4::operator * ( float f ) const
{
    return FVECTOR4(x * f, y * f, z * f, w * f);
}
FINLINE FVECTOR4 FVECTOR4::operator / ( float f ) const
{
    float fInv = 1.0f / f;
    return FVECTOR4(x * fInv, y * fInv, z * fInv, w * fInv);
}
FINLINE FVECTOR4 operator * ( float f, const FVECTOR4& v )
{
    return FVECTOR4(f * v.x, f * v.y, f * v.z, f * v.w);
}
FINLINE bool FVECTOR4::operator == ( const FVECTOR4& v ) const
{
    return x == v.x && y == v.y && z == v.z && w == v.w;
}
FINLINE bool FVECTOR4::operator != ( const FVECTOR4& v ) const
{
    return x != v.x || y != v.y || z != v.z || w != v.w;
}
FINLINE void FVECTOR4::Set(float fx, float fy, float fz, float fw)
{
    x = fx;
    y = fy;
    z = fz;
    w = fw;
}
FINLINE void FVECTOR4::Set(const FVECTOR3& v, float f)
{
    x = v.x;
    y = v.y;
    z = v.z;
    w = f;
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE FMATRIX::FMATRIX()
{
    FMatrixIdentity(this);
}
FINLINE FMATRIX::FMATRIX( const float* pf )
{
    FMemcpy(&_11, pf, sizeof(FMATRIX));
}
FINLINE FMATRIX::FMATRIX( const FMATRIX& mat )
{
    FMemcpy(&_11, &mat, sizeof(FMATRIX));
}
FINLINE FMATRIX::FMATRIX( float f11, float f12, float f13, float f14,
                        float f21, float f22, float f23, float f24,
                        float f31, float f32, float f33, float f34,
                        float f41, float f42, float f43, float f44 )
{
    _11 = f11; _12 = f12; _13 = f13; _14 = f14;
    _21 = f21; _22 = f22; _23 = f23; _24 = f24;
    _31 = f31; _32 = f32; _33 = f33; _34 = f34;
    _41 = f41; _42 = f42; _43 = f43; _44 = f44;
}
FINLINE FMATRIX::FMATRIX( FQUATERNION* qt )
{
    FMatrixRotationQuaternion(this, qt);
}
FINLINE float& FMATRIX::operator () ( UINT iRow, UINT iCol )
{
    return m[iRow][iCol];
}
FINLINE float FMATRIX::operator () ( UINT iRow, UINT iCol ) const
{
    return m[iRow][iCol];
}
FINLINE FMATRIX::operator float* ()
{
    return (float *) &_11;
}
FINLINE FMATRIX::operator const float* () const
{
    return (const float *) &_11;
}
FINLINE FMATRIX& FMATRIX::operator *= ( const FMATRIX& mat )
{
    FMatrixMultiply(this, this, &mat);
    return *this;
}
FINLINE FMATRIX& FMATRIX::operator += ( const FMATRIX& mat )
{
    _11 += mat._11; _12 += mat._12; _13 += mat._13; _14 += mat._14;
    _21 += mat._21; _22 += mat._22; _23 += mat._23; _24 += mat._24;
    _31 += mat._31; _32 += mat._32; _33 += mat._33; _34 += mat._34;
    _41 += mat._41; _42 += mat._42; _43 += mat._43; _44 += mat._44;
    return *this;
}
FINLINE FMATRIX& FMATRIX::operator -= ( const FMATRIX& mat )
{
    _11 -= mat._11; _12 -= mat._12; _13 -= mat._13; _14 -= mat._14;
    _21 -= mat._21; _22 -= mat._22; _23 -= mat._23; _24 -= mat._24;
    _31 -= mat._31; _32 -= mat._32; _33 -= mat._33; _34 -= mat._34;
    _41 -= mat._41; _42 -= mat._42; _43 -= mat._43; _44 -= mat._44;
    return *this;
}
FINLINE FMATRIX& FMATRIX::operator *= ( float f )
{
    _11 *= f; _12 *= f; _13 *= f; _14 *= f;
    _21 *= f; _22 *= f; _23 *= f; _24 *= f;
    _31 *= f; _32 *= f; _33 *= f; _34 *= f;
    _41 *= f; _42 *= f; _43 *= f; _44 *= f;
    return *this;
}
FINLINE FMATRIX& FMATRIX::operator /= ( float f )
{
    float fInv = 1.0f / f;
    _11 *= fInv; _12 *= fInv; _13 *= fInv; _14 *= fInv;
    _21 *= fInv; _22 *= fInv; _23 *= fInv; _24 *= fInv;
    _31 *= fInv; _32 *= fInv; _33 *= fInv; _34 *= fInv;
    _41 *= fInv; _42 *= fInv; _43 *= fInv; _44 *= fInv;
    return *this;
}
FINLINE FMATRIX FMATRIX::operator + () const
{
    return *this;
}
FINLINE FMATRIX FMATRIX::operator - () const
{
    return FMATRIX(-_11, -_12, -_13, -_14,
                      -_21, -_22, -_23, -_24,
                      -_31, -_32, -_33, -_34,
                      -_41, -_42, -_43, -_44);
}
FINLINE FMATRIX FMATRIX::operator * ( const FMATRIX& mat ) const
{
    FMATRIX matT;
    FMatrixMultiply(&matT, this, &mat);
    return matT;
}
FINLINE FMATRIX FMATRIX::operator + ( const FMATRIX& mat ) const
{
    return FMATRIX(_11 + mat._11, _12 + mat._12, _13 + mat._13, _14 + mat._14,
                      _21 + mat._21, _22 + mat._22, _23 + mat._23, _24 + mat._24,
                      _31 + mat._31, _32 + mat._32, _33 + mat._33, _34 + mat._34,
                      _41 + mat._41, _42 + mat._42, _43 + mat._43, _44 + mat._44);
}
FINLINE FMATRIX FMATRIX::operator - ( const FMATRIX& mat ) const
{
    return FMATRIX(_11 - mat._11, _12 - mat._12, _13 - mat._13, _14 - mat._14,
                      _21 - mat._21, _22 - mat._22, _23 - mat._23, _24 - mat._24,
                      _31 - mat._31, _32 - mat._32, _33 - mat._33, _34 - mat._34,
                      _41 - mat._41, _42 - mat._42, _43 - mat._43, _44 - mat._44);
}
FINLINE FMATRIX FMATRIX::operator * ( float f ) const
{
    return FMATRIX(_11 * f, _12 * f, _13 * f, _14 * f,
                      _21 * f, _22 * f, _23 * f, _24 * f,
                      _31 * f, _32 * f, _33 * f, _34 * f,
                      _41 * f, _42 * f, _43 * f, _44 * f);
}
FINLINE FMATRIX FMATRIX::operator / ( float f ) const
{
    float fInv = 1.0f / f;
    return FMATRIX(_11 * fInv, _12 * fInv, _13 * fInv, _14 * fInv,
                      _21 * fInv, _22 * fInv, _23 * fInv, _24 * fInv,
                      _31 * fInv, _32 * fInv, _33 * fInv, _34 * fInv,
                      _41 * fInv, _42 * fInv, _43 * fInv, _44 * fInv);
}
FINLINE FMATRIX operator * ( float f, const FMATRIX& mat )
{
    return FMATRIX(f * mat._11, f * mat._12, f * mat._13, f * mat._14,
                      f * mat._21, f * mat._22, f * mat._23, f * mat._24,
                      f * mat._31, f * mat._32, f * mat._33, f * mat._34,
                      f * mat._41, f * mat._42, f * mat._43, f * mat._44);
}
FINLINE bool FMATRIX::operator == ( const FMATRIX& mat ) const
{
    return 0 == FMemcmp(this, &mat, sizeof(FMATRIX));
}
FINLINE bool FMATRIX::operator != ( const FMATRIX& mat ) const
{
    return 0 != FMemcmp(this, &mat, sizeof(FMATRIX));
}
FINLINE FVECTOR3& FMATRIX::GetV3(size_t row)
{
    return *((FVECTOR3*)m[row]);
}
FINLINE FVECTOR4& FMATRIX::GetV4(size_t row)
{
    return *((FVECTOR4*)m[row]);
}
FINLINE bool FMATRIX::IsParity()
{
    FVECTOR3 c;
    FVec3Cross(&c, &GetV3(0), &GetV3(1));
    return FVec3Dot(&GetV3(2), &c) < 0.0f;
}
FINLINE FQUATERNION* FMATRIX::GetRotate(FQUATERNION* out)
{
    FVECTOR3 scale;
    GetScale(&scale);
    FMATRIX mr(
        scale.x, 0, 0, 0,
        0, scale.y, 0, 0,
        0, 0, scale.z, 0,
        0, 0, 0, 1);
    FMATRIX mo;
    if (!FMatrixInverse(&mo, 0, &mr))
        return 0;
    mr = mo * *this;
    return FQuaternionRotationMatrix(out, &mr);
}
FINLINE FVECTOR3* FMATRIX::GetScale(FVECTOR3* pOut)
{
    pOut->x = GetV3(0).Length();
    pOut->y = GetV3(1).Length();
    pOut->z = GetV3(2).Length();
    return pOut;
}
FINLINE FVECTOR3& FMATRIX::GetTranslation()
{
    return GetV3(3);
}
FINLINE void FMATRIX::RemoveRotate()
{
    _11 = 1.0f;	_12 = 0.0f; _13 = 0.0f;
    _21 = 0.0f;	_22 = 1.0f; _23 = 0.0f;
    _31 = 0.0f;	_32 = 0.0f;	_33 = 1.0f;
}
FINLINE void FMATRIX::RemoveScale()
{
    FVECTOR3 scale;
    GetScale(&scale);
    _11 /= scale.x;	_12 /= scale.x;	_13 /= scale.x;
    _21 /= scale.y;	_22 /= scale.y;	_23 /= scale.y;
    _31 /= scale.z;	_32 /= scale.z;	_33 /= scale.z;
}
FINLINE void FMATRIX::RemoveTranslation()
{
    _41 = 0.0f;
    _42 = 0.0f;
    _43 = 0.0f;
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE FQUATERNION::FQUATERNION( const float* pf )
{
    x = pf[0];
    y = pf[1];
    z = pf[2];
    w = pf[3];
}
FINLINE FQUATERNION::FQUATERNION( float fx, float fy, float fz, float fw )
{
    x = fx;
    y = fy;
    z = fz;
    w = fw;
}
FINLINE FQUATERNION::FQUATERNION( FVECTOR3* v, float fw )
{
    x = v->x;
    y = v->y;
    z = v->z;
    w = fw;
}
FINLINE FQUATERNION::FQUATERNION( FMATRIX* mat )
{
    FQuaternionRotationMatrix(this, mat);
}
FINLINE FQUATERNION::operator float* ()
{
    return (float *) &x;
}
FINLINE FQUATERNION::operator const float* () const
{
    return (const float *) &x;
}
FINLINE FQUATERNION& FQUATERNION::operator += ( const FQUATERNION& q )
{
    x += q.x;
    y += q.y;
    z += q.z;
    w += q.w;
    return *this;
}
FINLINE FQUATERNION& FQUATERNION::operator -= ( const FQUATERNION& q )
{
    x -= q.x;
    y -= q.y;
    z -= q.z;
    w -= q.w;
    return *this;
}
FINLINE FQUATERNION& FQUATERNION::operator *= ( const FQUATERNION& q )
{
    FQuaternionMultiply(this, this, &q);
    return *this;
}
FINLINE FQUATERNION& FQUATERNION::operator *= ( float f )
{
    x *= f;
    y *= f;
    z *= f;
    w *= f;
    return *this;
}
FINLINE FQUATERNION& FQUATERNION::operator /= ( float f )
{
    float fInv = 1.0f / f;
    x *= fInv;
    y *= fInv;
    z *= fInv;
    w *= fInv;
    return *this;
}
FINLINE FQUATERNION FQUATERNION::operator + () const
{
    return *this;
}
FINLINE FQUATERNION FQUATERNION::operator - () const
{
    return FQUATERNION(-x, -y, -z, -w);
}
FINLINE FQUATERNION FQUATERNION::operator + ( const FQUATERNION& q ) const
{
    return FQUATERNION(x + q.x, y + q.y, z + q.z, w + q.w);
}
FINLINE FQUATERNION FQUATERNION::operator - ( const FQUATERNION& q ) const
{
    return FQUATERNION(x - q.x, y - q.y, z - q.z, w - q.w);
}
FINLINE FQUATERNION FQUATERNION::operator * ( const FQUATERNION& q ) const
{
    FQUATERNION qT;
    FQuaternionMultiply(&qT, this, &q);
    return qT;
}
FINLINE FQUATERNION FQUATERNION::operator * ( float f ) const
{
    return FQUATERNION(x * f, y * f, z * f, w * f);
}
FINLINE FQUATERNION FQUATERNION::operator / ( float f ) const
{
    float fInv = 1.0f / f;
    return FQUATERNION(x * fInv, y * fInv, z * fInv, w * fInv);
}
FINLINE FQUATERNION operator * (float f, const FQUATERNION& q )
{
    return FQUATERNION(f * q.x, f * q.y, f * q.z, f * q.w);
}
FINLINE bool FQUATERNION::operator == ( const FQUATERNION& q ) const
{
    return x == q.x && y == q.y && z == q.z && w == q.w;
}
FINLINE bool FQUATERNION::operator != ( const FQUATERNION& q ) const
{
    return x != q.x || y != q.y || z != q.z || w != q.w;
}
FINLINE FVECTOR3* FQUATERNION::GetVirtual(FVECTOR3* out)
{
    out->x = x;
    out->y = y;
    out->z = z;
    return out;
}
FINLINE void FQUATERNION::Identity()
{
    x = y = z = 0;
    w = 1;
}
FINLINE float FQUATERNION::Yaw()
{
    return atan2f(2.0f * (x * z + y * w), w * w - x * x - y * y + z * z);
}
FINLINE float FQUATERNION::Pitch()
{
    return asinf(-2.0f * (y * z - x * w));
}
FINLINE float FQUATERNION::Roll()
{
    return atan2f(2.0f * (x * y + z * w), w * w - x * x + y * y - z * z);
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE FTRIANGLE::FTRIANGLE(FVECTOR3* p0, FVECTOR3* p1, FVECTOR3* p2)
    :v0(*p0)
    ,v1(*p1)
    ,v2(*p2)
{}
FINLINE bool FTRIANGLE::HitRay(FRAY* ray, FVECTOR3* hit_postion)
{
    float d;
    if (FIntersectTri(&v0, &v1, &v2, &ray->origin, &ray->dir, &d))
    {
        if (hit_postion)
            ray->DstPoint(d, hit_postion);
        return true;
    }
    return false;
}
FINLINE FVECTOR3* FTRIANGLE::GetNormal(FVECTOR3* out)
{
    FVECTOR3 p1 = v1 - v0;
    FVECTOR3 p2 = v2 - v0;
    return FVec3Cross(out, &p1, &p2);
}
FINLINE bool FTRIANGLE::IsSameSide(const FVECTOR3& p1, const FVECTOR3& p2, const FVECTOR3& a, const FVECTOR3& b)
{
    FVECTOR3 a2b = b - a;
    FVECTOR3 c1;
    FVECTOR3 k1 = p1 - a;
    FVec3Cross(&c1, &a2b, &k1);
    FVECTOR3 c2;
    FVECTOR3 k2 = p2 - a;
    FVec3Cross(&c2, &a2b, &k2);
    return (FVec3Dot(&c1, &c2) >= 0.0f);
}
FINLINE bool FTRIANGLE::IsPointInside(const FVECTOR3& p)
{
    return (IsSameSide(p, v0, v1, v2) &&
            IsSameSide(p, v1, v2, v0) &&
            IsSameSide(p, v2, v0, v1));
}
FINLINE void FTRIANGLE::operator = (const FTRIANGLE& b)
{
    v0 = b.v0;
    v1 = b.v1;
    v2 = b.v2;
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE FSPHERE::FSPHERE(FVECTOR3* c, float r)
    :center(*c)
    ,radius(r)
{}
FINLINE bool FSPHERE::HitAABB(FAABB* aabb)
{
    return FHitAABB_sphere(aabb, this);
}
FINLINE bool FSPHERE::HitFrustum(FFRUSTUM* f)
{
    return FHitFrustum_sphere(f, this);
}
FINLINE void FSPHERE::operator = (const FSPHERE& b)
{
    center = b.center;
    radius = b.radius;
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE FRAY::FRAY(FVECTOR3* p, FVECTOR3* d)
    :origin(*p)
    ,dir(*d)
{}
FINLINE FVECTOR3* FRAY::DstPoint(float distance, FVECTOR3* dst, bool flip)
{
    *dst = origin + dir * (flip ? -distance : distance);
    return dst;
}
FINLINE bool FRAY::HitAABB(FAABB* aabb)
{
    return FHitAABB_ray(aabb, this);
}
FINLINE bool FRAY::HitFrustum(FFRUSTUM* f, FVECTOR3* hit_position)
{
    return FHitFrustum_ray(f, this, hit_position);
}
FINLINE bool FRAY::HitPlane(FPLANE* plane, FVECTOR3* hit_position)
{
    FVECTOR3 dst;
    return FPlaneIntersectLine(hit_position, plane, &origin, DstPoint(99999999.0f, &dst)) != 0;
}
FINLINE void FRAY::operator = (const FRAY& b)
{
    origin = b.origin;
    dir = b.dir;
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE FAABB::FAABB(FVECTOR3* vmin, FVECTOR3* vmax)
{
    vMin = *vmin;
    vMax = *vmax;
}
FINLINE FAABB::FAABB(const FAABB& b)
{
    vMin = b.vMin;
    vMax = b.vMax;
}
FINLINE bool FAABB::Empty() const
{
#if 1
    return (vMin.x > vMax.x ||  vMin.y > vMax.y || vMin.z > vMax.z);
#else
    return (vMin.x ==  F_FLT_MAX && vMin.x == vMin.y && vMin.z == vMin.y) &&
           (vMax.x == -F_FLT_MAX && vMax.x == vMax.y && vMax.z == vMax.y);
#endif
}
FINLINE void FAABB::Clear()
{
    vMin.x =  F_FLT_MAX;
    vMin.y =  F_FLT_MAX;
    vMin.z =  F_FLT_MAX;
    vMax.x = -F_FLT_MAX;
    vMax.y = -F_FLT_MAX;
    vMax.z = -F_FLT_MAX;
}
FINLINE void FAABB::ToPoint(FVECTOR3* vtx)
{
    vtx[0].x = vMin.x;
    vtx[0].y = vMin.y;
    vtx[0].z = vMin.z;

    vtx[1].x = vMin.x;
    vtx[1].y = vMin.y;
    vtx[1].z = vMax.z;

    vtx[2].x = vMin.x;
    vtx[2].y = vMax.y;
    vtx[2].z = vMax.z;

    vtx[3].x = vMin.x;
    vtx[3].y = vMax.y;
    vtx[3].z = vMin.z;

    vtx[4].x = vMax.x;
    vtx[4].y = vMin.y;
    vtx[4].z = vMin.z;

    vtx[5].x = vMax.x;
    vtx[5].y = vMin.y;
    vtx[5].z = vMax.z;

    vtx[6].x = vMax.x;
    vtx[6].y = vMax.y;
    vtx[6].z = vMax.z;

    vtx[7].x = vMax.x;
    vtx[7].y = vMax.y;
    vtx[7].z = vMin.z;
}
FINLINE FAABB& FAABB::Push(FVECTOR3* v)
{
    if (v->x < vMin.x)
        vMin.x = v->x;
    if (v->y < vMin.y)
        vMin.y = v->y;
    if (v->z < vMin.z)
        vMin.z = v->z;
    if (v->x > vMax.x)
        vMax.x = v->x;
    if (v->y > vMax.y)
        vMax.y = v->y;
    if (v->z > vMax.z)
        vMax.z = v->z;
    return *this;
}
FINLINE void FAABB::Merge(FAABB* b)
{
    if (b->Empty())
        return;
    if (b->vMin.x < vMin.x)
        vMin.x = b->vMin.x;
    if (b->vMin.x > vMax.x)
        vMax.x = b->vMin.x;
    if (b->vMin.y < vMin.y)
        vMin.y = b->vMin.y;
    if (b->vMin.y > vMax.y)
        vMax.y = b->vMin.y;
    if (b->vMin.z < vMin.z)
        vMin.z = b->vMin.z;
    if (b->vMin.z > vMax.z)
        vMax.z = b->vMin.z;
}
FINLINE FAABB& FAABB::Set(FVECTOR3* _min, FVECTOR3* _max)
{
    vMin = *_min;
    vMax = *_max;
    return *this;
}
FINLINE bool FAABB::HitPoint(FVECTOR3* pos)
{
    return FHitAABB_dot(this, pos);
}
FINLINE bool FAABB::HitSphere(FSPHERE* sphere)
{
    return FHitAABB_sphere(this, sphere);
}
FINLINE bool FAABB::HitRay(FRAY* ray)
{
    return FHitAABB_ray(this, ray);
}
FINLINE bool FAABB::HitAABB(FAABB* b, FAABB* intersection)
{
    return FHitAABB_aabb(this, b, intersection);
}
FINLINE bool FAABB::HitFrustum(FFRUSTUM* f)
{
    return FHitFrustum_aabb(f, this);
}
FINLINE EF_INTERSECTION_TYPE FAABB::HitPlane(FPLANE* plane)
{
    return FHitAABB_plane(this, plane);
}
FINLINE EF_INTERSECTION_TYPE FAABB::HitPlane(FVECTOR3* p, FVECTOR3* n)
{
    return FHitAABB_plane2(this, p, n);
}
FINLINE void FAABB::GetNear(FVECTOR3* p, FVECTOR3* out)
{
    if (p->x < vMin.x)
        out->x = vMin.x;
    else if (p->x > vMax.x)
        out->x = vMax.x;
    else
        out->x = p->x;

    if (p->y < vMin.y) 
        out->y = vMin.y;
    else if (p->y > vMax.y) 
        out->y = vMax.y;
    else
        out->y = p->y;

    if (p->z < vMin.z)
        out->z = vMin.z;
    else if (p->z > vMax.z)
        out->z = vMax.z;
    else
        out->z = p->z;
}
FINLINE FAABB& FAABB::Inflate(float v)
{
    return Inflate(v, v, v);
}
FINLINE FAABB& FAABB::Inflate(FVECTOR3* i)
{
    return Inflate(i->x, i->y, i->z);
}
FINLINE FAABB& FAABB::Inflate(float x, float y, float z)
{
    vMin.x -= x;
    vMax.x += x;
    vMin.y -= y;
    vMax.y += y;
    vMin.z -= z;
    vMax.z += z;
    return *this;
}
FINLINE float FAABB::LengthX()
{
    return vMax.x - vMin.x;
}
FINLINE float FAABB::LengthY()
{
    return vMax.y - vMin.y;
}
FINLINE float FAABB::LengthZ()
{
    return vMax.z - vMin.z;
}
FINLINE FVECTOR3* FAABB::Length(FVECTOR3* ret)
{
    ret->x = LengthX();
    ret->y = LengthY();
    ret->z = LengthZ();
    return ret;
}

FINLINE void FAABB::operator = (const FAABB& b)
{
    vMin = b.vMin;
    vMax = b.vMax;
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE FAABB_CENTER::FAABB_CENTER(FAABB* aabb)
{
    vMin = aabb->vMin;
    vMax = aabb->vMax;
    MakeCenter();
}
FINLINE FVECTOR3& FAABB_CENTER::MakeCenter()
{
    vCenter.x = (vMax.x + vMin.x) / 2.0f;
    vCenter.y = (vMax.y + vMin.y) / 2.0f;
    vCenter.z = (vMax.z + vMin.z) / 2.0f;
    return vCenter;
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE FPLANE::FPLANE( const float* pf )
{
    a = pf[0];
    b = pf[1];
    c = pf[2];
    d = pf[3];
}
FINLINE FPLANE::FPLANE( float fa, float fb, float fc, float fd )
{
    a = fa;
    b = fb;
    c = fc;
    d = fd;
}
FINLINE FPLANE::operator float* ()
{
    return (float *) &a;
}
FINLINE FPLANE::operator const float* () const
{
    return (const float *) &a;
}
FINLINE FPLANE& FPLANE::operator *= ( float f )
{
    a *= f;
    b *= f;
    c *= f;
    d *= f;
    return *this;
}
FINLINE FPLANE& FPLANE::operator /= ( float f )
{
    float fInv = 1.0f / f;
    a *= fInv;
    b *= fInv;
    c *= fInv;
    d *= fInv;
    return *this;
}
FINLINE FPLANE FPLANE::operator + () const
{
    return *this;
}
FINLINE FPLANE FPLANE::operator - () const
{
    return FPLANE(-a, -b, -c, -d);
}
FINLINE FPLANE FPLANE::operator * ( float f ) const
{
    return FPLANE(a * f, b * f, c * f, d * f);
}
FINLINE FPLANE FPLANE::operator / ( float f ) const
{
    float fInv = 1.0f / f;
    return FPLANE(a * fInv, b * fInv, c * fInv, d * fInv);
}
FINLINE FPLANE operator * (float f, const FPLANE& p )
{
    return FPLANE(f * p.a, f * p.b, f * p.c, f * p.d);
}
FINLINE bool FPLANE::operator == ( const FPLANE& p ) const
{
    return a == p.a && b == p.b && c == p.c && d == p.d;
}
FINLINE bool FPLANE::operator != ( const FPLANE& p ) const
{
    return a != p.a || b != p.b || c != p.c || d != p.d;
}
FINLINE FVECTOR3& FPLANE::GetNormal()
{
    return *((FVECTOR3*)this);
}
FINLINE float FPLANE::GetDistance()
{
    return d;
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE FFRUSTUM::FFRUSTUM(FMATRIX* view, FMATRIX* proj, float distance)
{
    FFrustumBuild(this, view, proj, distance);
}
FINLINE void FFRUSTUM::Make(FMATRIX* view, FMATRIX* proj, float distance)
{
    FFrustumBuild(this, view, proj, distance);
}
FINLINE void FFRUSTUM::Make(FVECTOR3* vp)
{
    FFrustumBuild_vp(this, vp);
}
FINLINE bool FFRUSTUM::HitPoint(FVECTOR3* p)
{
    return FHitFrustum_dot(this, p);
}
FINLINE bool FFRUSTUM::HitShpere(FSPHERE* sphere)
{
    return FHitFrustum_sphere(this, sphere);
}
FINLINE bool FFRUSTUM::HitRay(FRAY* ray, FVECTOR3* hit_position)
{
    return FHitFrustum_ray(this, ray, hit_position);
}
FINLINE bool FFRUSTUM::HitAABB(FAABB* aabb)
{
    return FHitFrustum_aabb(this, aabb);
}
FINLINE EF_INTERSECTION_TYPE FFRUSTUM::HitPlane(FPLANE* plane)
{
    return FHitFrustum_plane(this, plane);
}
FINLINE EF_INTERSECTION_TYPE FFRUSTUM::HitPlane(FVECTOR3* p, FVECTOR3* n)
{
    return FHitFrustum_plane2(this, p, n);
}
FINLINE void FFRUSTUM::operator = (const FFRUSTUM& b)
{
    FMemcpy(plane, b.plane, sizeof(plane));
    FMemcpy(position, b.position, sizeof(position));
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
template < size_t mem_count >
FINLINE FBIT_OPERATION< mem_count >::FBIT_OPERATION()
{
    set_all(false);
}
template < size_t mem_count >
FINLINE void FBIT_OPERATION< mem_count >::set_all(bool one)
{
    memset(data, one ? 0xff : 0, sizeof(data));
}
template < size_t mem_count >
FINLINE void FBIT_OPERATION< mem_count >::set(size_t bit_position, bool one)
{
    if (bit_position >= 0 && bit_position < (mem_count<<3))
    {
        size_t idx = bit_position>>3;
        BYTE mark = 1<<(bit_position - (idx<<3));
        if (one)
            data[idx] |= mark;
        else
            data[idx] &= ~mark;
    }
}
template < size_t mem_count >
FINLINE bool FBIT_OPERATION< mem_count >::get(size_t bit_position)
{
    if (bit_position >= 0 && bit_position < (mem_count<<3))
    {
        size_t idx = bit_position>>3;
        BYTE mark = 1<<(bit_position - (idx<<3));
        return (data[idx] & mark) != 0;
    }
    return false;
}
template < size_t mem_count >
FINLINE bool FBIT_OPERATION< mem_count >::operator [] (size_t bit_position)
{
    return get(bit_position);
}
template < size_t mem_count >
FINLINE bool FBIT_OPERATION< mem_count >::operator == (const FBIT_OPERATION<mem_count>& b) const
{
    return (FMemcmp(data, b.data, sizeof(data)) == 0);
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE FCOLOR::FCOLOR( DWORD dw )
{
    SetHWColor(dw);
}
FINLINE FCOLOR::FCOLOR( const float* pf )
{
    r = pf[0];
    g = pf[1];
    b = pf[2];
    a = pf[3];
}
FINLINE FCOLOR::FCOLOR( float fr, float fg, float fb, float fa )
{
    r = fr;
    g = fg;
    b = fb;
    a = fa;
}
FINLINE FCOLOR::operator DWORD () const
{
    return FARGBF(a, r, g, b);
}
FINLINE FCOLOR::operator float * ()
{
    return (float *) &r;
}
FINLINE FCOLOR::operator const float * () const
{
    return (const float *) &r;
}
FINLINE FCOLOR& FCOLOR::operator += ( const FCOLOR& c )
{
    r += c.r;
    g += c.g;
    b += c.b;
    a += c.a;
    return *this;
}
FINLINE FCOLOR& FCOLOR::operator -= ( const FCOLOR& c )
{
    r -= c.r;
    g -= c.g;
    b -= c.b;
    a -= c.a;
    return *this;
}
FINLINE FCOLOR& FCOLOR::operator *= ( float f )
{
    r *= f;
    g *= f;
    b *= f;
    a *= f;
    return *this;
}
FINLINE FCOLOR& FCOLOR::operator /= ( float f )
{
    float fInv = 1.0f / f;
    r *= fInv;
    g *= fInv;
    b *= fInv;
    a *= fInv;
    return *this;
}
FINLINE FCOLOR FCOLOR::operator + () const
{
    return *this;
}
FINLINE FCOLOR FCOLOR::operator - () const
{
    return FCOLOR(-r, -g, -b, -a);
}
FINLINE FCOLOR FCOLOR::operator + ( const FCOLOR& c ) const
{
    return FCOLOR(r + c.r, g + c.g, b + c.b, a + c.a);
}
FINLINE FCOLOR FCOLOR::operator - ( const FCOLOR& c ) const
{
    return FCOLOR(r - c.r, g - c.g, b - c.b, a - c.a);
}
FINLINE FCOLOR FCOLOR::operator * ( float f ) const
{
    return FCOLOR(r * f, g * f, b * f, a * f);
}
FINLINE FCOLOR FCOLOR::operator / ( float f ) const
{
    float fInv = 1.0f / f;
    return FCOLOR(r * fInv, g * fInv, b * fInv, a * fInv);
}
FINLINE FCOLOR operator * (float f, const FCOLOR& c )
{
    return FCOLOR(f * c.r, f * c.g, f * c.b, f * c.a);
}
FINLINE bool FCOLOR::operator == ( const FCOLOR& c ) const
{
    return r == c.r && g == c.g && b == c.b && a == c.a;
}
FINLINE bool FCOLOR::operator != ( const FCOLOR& c ) const
{
    return r != c.r || g != c.g || b != c.b || a != c.a;
}
FINLINE void FCOLOR::Clamp()
{
    FColorClamp(r);
    FColorClamp(g);
    FColorClamp(b);
    FColorClamp(a);
}
FINLINE void FCOLOR::SetHWColor(DWORD col)
{
    a = FGetAF(col);
    r = FGetRF(col);
    g = FGetGF(col);
    b = FGetBF(col);
}
FINLINE DWORD FCOLOR::GetHWColor() const
{
    return FARGBF(a, r, g, b);
}
FINLINE void FCOLOR::Set(float fr, float fg, float fb, float fa)
{
    r = fr;
    g = fg;
    b = fb;
    a = fa;
}
FINLINE FCOLOR* FCOLOR::Set(FVECTOR3* in)
{
    r = in->x;
    g = in->y;
    b = in->z;
    return this;
}
FINLINE FCOLOR* FCOLOR::Set(FVECTOR4* in, bool with_alpha)
{
    Set((FVECTOR3*)in);
    if (with_alpha)
        a = in->w;
    return this;
}
FINLINE void FCOLOR::Get(FVECTOR3* out)
{
    out->x = r;
    out->y = g;
    out->z = b;
}
FINLINE void FCOLOR::Get(FVECTOR4* out, bool with_alpha)
{
    Get((FVECTOR3*)out);
    if (with_alpha)
        out->w = a;
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE float FVec2Length( const FVECTOR2 *pV )
{
    return FSqrt(pV->x * pV->x + pV->y * pV->y);
}
FINLINE float FVec2LengthSq( const FVECTOR2 *pV )
{
    return pV->x * pV->x + pV->y * pV->y;
}
FINLINE float FVec2Dot( const FVECTOR2 *pV1, const FVECTOR2 *pV2 )
{
    return pV1->x * pV2->x + pV1->y * pV2->y;
}
FINLINE float FVec2CCW( const FVECTOR2 *pV1, const FVECTOR2 *pV2 )
{
    return pV1->x * pV2->y - pV1->y * pV2->x;
}
FINLINE FVECTOR2* FVec2Add( FVECTOR2 *pOut, const FVECTOR2 *pV1, const FVECTOR2 *pV2 )
{
    pOut->x = pV1->x + pV2->x;
    pOut->y = pV1->y + pV2->y;
    return pOut;
}
FINLINE FVECTOR2* FVec2Subtract( FVECTOR2 *pOut, const FVECTOR2 *pV1, const FVECTOR2 *pV2 )
{
    pOut->x = pV1->x - pV2->x;
    pOut->y = pV1->y - pV2->y;
    return pOut;
}
FINLINE FVECTOR2* FVec2Minimize( FVECTOR2 *pOut, const FVECTOR2 *pV1, const FVECTOR2 *pV2 )
{
    pOut->x = pV1->x < pV2->x ? pV1->x : pV2->x;
    pOut->y = pV1->y < pV2->y ? pV1->y : pV2->y;
    return pOut;
}
FINLINE FVECTOR2* FVec2Maximize( FVECTOR2 *pOut, const FVECTOR2 *pV1, const FVECTOR2 *pV2 )
{
    pOut->x = pV1->x > pV2->x ? pV1->x : pV2->x;
    pOut->y = pV1->y > pV2->y ? pV1->y : pV2->y;
    return pOut;
}
FINLINE FVECTOR2* FVec2Scale( FVECTOR2 *pOut, const FVECTOR2 *pV, float s )
{
    pOut->x = pV->x * s;
    pOut->y = pV->y * s;
    return pOut;
}
FINLINE FVECTOR2* FVec2Lerp( FVECTOR2 *pOut, const FVECTOR2 *pV1, const FVECTOR2 *pV2, float s )
{
    pOut->x = pV1->x + s * (pV2->x - pV1->x);
    pOut->y = pV1->y + s * (pV2->y - pV1->y);
    return pOut;
}
FINLINE FVECTOR2* FVec2Normalize( FVECTOR2 *pOut, const FVECTOR2 *pV )
{
    float l = FVec2Length(pV);
    if (l > 0)
    {
        pOut->x /= l;
        pOut->y /= l;
    }
    return pOut;
}
FINLINE bool FVec2Angle( FVECTOR2 *pA, const FVECTOR2 *pB, float& angle )
{
    if (!pA)
        return false;

    if (!pB)
    {
        angle = atan2f(pA->y, pA->x);
        return true;
    }

    FVECTOR2 s = *pA, t = *pB;

    FVec2Normalize(&s, &s);
    FVec2Normalize(&t, &t);

    angle = acosf(FVec2Dot(&s, &t));

    return true;
}
FINLINE FVECTOR2* FVec2Rotate( FVECTOR2 *pV, float a )
{
    FVECTOR2 v;

    v.x = pV->x * cosf(a) - pV->y * sinf(a);
    v.y = pV->x * sinf(a) + pV->y * cosf(a);

    pV->x = v.x, pV->y = v.y;

    return pV;
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE float FVec3Length( const FVECTOR3 *pV )
{
    return FSqrt(pV->x * pV->x + pV->y * pV->y + pV->z * pV->z);
}
FINLINE float FVec3LengthSq( const FVECTOR3 *pV )
{
    return pV->x * pV->x + pV->y * pV->y + pV->z * pV->z;
}
FINLINE float FVec3Dot( const FVECTOR3 *pV1, const FVECTOR3 *pV2 )
{
    return pV1->x * pV2->x + pV1->y * pV2->y + pV1->z * pV2->z;
}
FINLINE FVECTOR3* FVec3Cross( FVECTOR3 *pOut, const FVECTOR3 *pV1, const FVECTOR3 *pV2 )
{
    FVECTOR3 v;
    v.x = pV1->y * pV2->z - pV1->z * pV2->y;
    v.y = pV1->z * pV2->x - pV1->x * pV2->z;
    v.z = pV1->x * pV2->y - pV1->y * pV2->x;
    *pOut = v;
    return pOut;
}
FINLINE FVECTOR3* FVec3Add( FVECTOR3 *pOut, const FVECTOR3 *pV1, const FVECTOR3 *pV2 )
{
    pOut->x = pV1->x + pV2->x;
    pOut->y = pV1->y + pV2->y;
    pOut->z = pV1->z + pV2->z;
    return pOut;
}
FINLINE FVECTOR3* FVec3Subtract( FVECTOR3 *pOut, const FVECTOR3 *pV1, const FVECTOR3 *pV2 )
{
    pOut->x = pV1->x - pV2->x;
    pOut->y = pV1->y - pV2->y;
    pOut->z = pV1->z - pV2->z;
    return pOut;
}
FINLINE FVECTOR3* FVec3Minimize( FVECTOR3 *pOut, const FVECTOR3 *pV1, const FVECTOR3 *pV2 )
{
    pOut->x = pV1->x < pV2->x ? pV1->x : pV2->x;
    pOut->y = pV1->y < pV2->y ? pV1->y : pV2->y;
    pOut->z = pV1->z < pV2->z ? pV1->z : pV2->z;
    return pOut;
}
FINLINE FVECTOR3* FVec3Maximize( FVECTOR3 *pOut, const FVECTOR3 *pV1, const FVECTOR3 *pV2 )
{
    pOut->x = pV1->x > pV2->x ? pV1->x : pV2->x;
    pOut->y = pV1->y > pV2->y ? pV1->y : pV2->y;
    pOut->z = pV1->z > pV2->z ? pV1->z : pV2->z;
    return pOut;
}
FINLINE FVECTOR3* FVec3Scale( FVECTOR3 *pOut, const FVECTOR3 *pV, float s)
{
    pOut->x = pV->x * s;
    pOut->y = pV->y * s;
    pOut->z = pV->z * s;
    return pOut;
}
FINLINE FVECTOR3* FVec3Lerp( FVECTOR3 *pOut, const FVECTOR3 *pV1, const FVECTOR3 *pV2, float s )
{
    pOut->x = pV1->x + s * (pV2->x - pV1->x);
    pOut->y = pV1->y + s * (pV2->y - pV1->y);
    pOut->z = pV1->z + s * (pV2->z - pV1->z);
    return pOut;
}
FINLINE FVECTOR3* FVec3Normalize( FVECTOR3 *pOut, const FVECTOR3 *pV )
{
    float l = FVec3Length(pV);
    if (l > 0)
    {
        pOut->x = pV->x / l;
        pOut->y = pV->y / l;
        pOut->z = pV->z / l;
    }
    else if (pOut != pV)
    {
        pOut->x = pV->x;
        pOut->y = pV->y;
        pOut->z = pV->z;
    }
    return pOut;
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE float FVec4Length( const FVECTOR4 *pV )
{
    return FSqrt(pV->x * pV->x + pV->y * pV->y + pV->z * pV->z + pV->w * pV->w);
}
FINLINE float FVec4LengthSq( const FVECTOR4 *pV )
{
    return pV->x * pV->x + pV->y * pV->y + pV->z * pV->z + pV->w * pV->w;
}
FINLINE float FVec4Dot( const FVECTOR4 *pV1, const FVECTOR4 *pV2 )
{
    return pV1->x * pV2->x + pV1->y * pV2->y + pV1->z * pV2->z + pV1->w * pV2->w;
}
FINLINE FVECTOR4* FVec4Add( FVECTOR4 *pOut, const FVECTOR4 *pV1, const FVECTOR4 *pV2)
{
    pOut->x = pV1->x + pV2->x;
    pOut->y = pV1->y + pV2->y;
    pOut->z = pV1->z + pV2->z;
    pOut->w = pV1->w + pV2->w;
    return pOut;
}
FINLINE FVECTOR4* FVec4Subtract( FVECTOR4 *pOut, const FVECTOR4 *pV1, const FVECTOR4 *pV2)
{
    pOut->x = pV1->x - pV2->x;
    pOut->y = pV1->y - pV2->y;
    pOut->z = pV1->z - pV2->z;
    pOut->w = pV1->w - pV2->w;
    return pOut;
}
FINLINE FVECTOR4* FVec4Minimize( FVECTOR4 *pOut, const FVECTOR4 *pV1, const FVECTOR4 *pV2)
{
    pOut->x = pV1->x < pV2->x ? pV1->x : pV2->x;
    pOut->y = pV1->y < pV2->y ? pV1->y : pV2->y;
    pOut->z = pV1->z < pV2->z ? pV1->z : pV2->z;
    pOut->w = pV1->w < pV2->w ? pV1->w : pV2->w;
    return pOut;
}
FINLINE FVECTOR4* FVec4Maximize( FVECTOR4 *pOut, const FVECTOR4 *pV1, const FVECTOR4 *pV2)
{
    pOut->x = pV1->x > pV2->x ? pV1->x : pV2->x;
    pOut->y = pV1->y > pV2->y ? pV1->y : pV2->y;
    pOut->z = pV1->z > pV2->z ? pV1->z : pV2->z;
    pOut->w = pV1->w > pV2->w ? pV1->w : pV2->w;
    return pOut;
}
FINLINE FVECTOR4* FVec4Scale( FVECTOR4 *pOut, const FVECTOR4 *pV, float s)
{
    pOut->x = pV->x * s;
    pOut->y = pV->y * s;
    pOut->z = pV->z * s;
    pOut->w = pV->w * s;
    return pOut;
}
FINLINE FVECTOR4* FVec4Lerp( FVECTOR4 *pOut, const FVECTOR4 *pV1, const FVECTOR4 *pV2, float s )
{
    pOut->x = pV1->x + s * (pV2->x - pV1->x);
    pOut->y = pV1->y + s * (pV2->y - pV1->y);
    pOut->z = pV1->z + s * (pV2->z - pV1->z);
    pOut->w = pV1->w + s * (pV2->w - pV1->w);
    return pOut;
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE bool FMatrixIsIdentity( const FMATRIX *pM )
{
    return pM->m[0][0] == 1.0f && pM->m[0][1] == 0.0f && pM->m[0][2] == 0.0f && pM->m[0][3] == 0.0f &&
           pM->m[1][0] == 0.0f && pM->m[1][1] == 1.0f && pM->m[1][2] == 0.0f && pM->m[1][3] == 0.0f &&
           pM->m[2][0] == 0.0f && pM->m[2][1] == 0.0f && pM->m[2][2] == 1.0f && pM->m[2][3] == 0.0f &&
           pM->m[3][0] == 0.0f && pM->m[3][1] == 0.0f && pM->m[3][2] == 0.0f && pM->m[3][3] == 1.0f;
}
//FINLINE FMATRIX* FMatrixMultiply( FMATRIX *pOut, const FMATRIX *pM1, const FMATRIX *pM2 )
//{
//    for (int i = 0; i < 4; i++)
//    {
//        float mi0 = pM1->m[i][0];
//        float mi1 = pM1->m[i][1];
//        float mi2 = pM1->m[i][2];
//        pOut->m[i][0] = mi0*pM2->m[0][0] + mi1*pM2->m[1][0] + mi2*pM2->m[2][0];
//        pOut->m[i][1] = mi0*pM2->m[0][1] + mi1*pM2->m[1][1] + mi2*pM2->m[2][1];
//        pOut->m[i][2] = mi0*pM2->m[0][2] + mi1*pM2->m[1][2] + mi2*pM2->m[2][2];
//    }
//    pOut->m[3][0] += pM2->m[3][0];
//    pOut->m[3][1] += pM2->m[3][1];
//    pOut->m[3][2] += pM2->m[3][2];
//    pOut->m[0][3] = 0.0f;
//    pOut->m[1][3] = 0.0f;
//    pOut->m[2][3] = 0.0f;
//    pOut->m[3][3] = 1.0f;
//    return pOut;
//}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE float FQuaternionLength( const FQUATERNION *pQ )
{
    return FSqrt(pQ->x * pQ->x + pQ->y * pQ->y + pQ->z * pQ->z + pQ->w * pQ->w);
}
FINLINE FQUATERNION* FQuaternionScale(FQUATERNION* pOut, const FQUATERNION* pIn, FLOAT s)
{
    pOut->x = pIn->x * s;
    pOut->y = pIn->y * s;
    pOut->z = pIn->z * s;
    pOut->w = pIn->w * s;
    return pOut;
}
FINLINE FQUATERNION* FQuaternionAdd(FQUATERNION* pOut, const FQUATERNION* pQ1, const FQUATERNION* pQ2)
{
    pOut->x = pQ1->x + pQ2->x;
    pOut->y = pQ1->y + pQ2->y;
    pOut->z = pQ1->z + pQ2->z;
    pOut->w = pQ1->w + pQ2->w;
    return pOut;
}
FINLINE FQUATERNION* FQuaternionNormalize(FQUATERNION* pOut, const FQUATERNION* pIn)
{
    FLOAT length = FQuaternionLength(pIn);
    FQuaternionScale(pOut, pIn, 1.0f / length);
    return pOut;
}
FINLINE float FQuaternionLengthSq( const FQUATERNION *pQ )
{
    return pQ->x * pQ->x + pQ->y * pQ->y + pQ->z * pQ->z + pQ->w * pQ->w;
}
FINLINE float FQuaternionDot( const FQUATERNION *pQ1, const FQUATERNION *pQ2 )
{
    return pQ1->x * pQ2->x + pQ1->y * pQ2->y + pQ1->z * pQ2->z + pQ1->w * pQ2->w;
}
FINLINE FQUATERNION* FQuaternionIdentity( FQUATERNION *pOut )
{
    pOut->x = pOut->y = pOut->z = 0.0f;
    pOut->w = 1.0f;
    return pOut;
}
FINLINE bool FQuaternionIsIdentity( const FQUATERNION *pQ )
{
    return pQ->x == 0.0f && pQ->y == 0.0f && pQ->z == 0.0f && pQ->w == 1.0f;
}
FINLINE FQUATERNION* FQuaternionConjugate( FQUATERNION *pOut, const FQUATERNION *pQ )
{
    pOut->x = -pQ->x;
    pOut->y = -pQ->y;
    pOut->z = -pQ->z;
    pOut->w =  pQ->w;
    return pOut;
}
FINLINE FQUATERNION* FQuaternionMultiply( FQUATERNION *pOut, const FQUATERNION *pQ1, const FQUATERNION *pQ2 )
{
    float qx = pQ1->w*pQ2->x + pQ1->x*pQ2->w + pQ1->y*pQ2->z - pQ1->z*pQ2->y;
    float qy = pQ1->w*pQ2->y + pQ1->y*pQ2->w + pQ1->z*pQ2->x - pQ1->x*pQ2->z;
    float qz = pQ1->w*pQ2->z + pQ1->z*pQ2->w + pQ1->x*pQ2->y - pQ1->y*pQ2->x;
    float qw = pQ1->w*pQ2->w - pQ1->x*pQ2->x - pQ1->y*pQ2->y - pQ1->z*pQ2->z;
    pOut->x = qx;
    pOut->y = qy;
    pOut->z = qz;
    pOut->w = qw;
    return pOut;
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE float FPlaneDot( const FPLANE *pP, const FVECTOR4 *pV)
{
    return pP->a * pV->x + pP->b * pV->y + pP->c * pV->z + pP->d * pV->w;
}
FINLINE float FPlaneDotCoord( const FPLANE *pP, const FVECTOR3 *pV)
{
    return pP->a * pV->x + pP->b * pV->y + pP->c * pV->z + pP->d;
}
FINLINE float FPlaneDotNormal( const FPLANE *pP, const FVECTOR3 *pV)
{
    return pP->a * pV->x + pP->b * pV->y + pP->c * pV->z;
}
FINLINE FPLANE* FPlaneScale(FPLANE *pOut, const FPLANE *pP, float s)
{
    pOut->a = pP->a * s;
    pOut->b = pP->b * s;
    pOut->c = pP->c * s;
    pOut->d = pP->d * s;
    return pOut;
}


//----------------------------------------------------------------
//
//----------------------------------------------------------------
FINLINE FCOLOR* FColorNegative(FCOLOR *pOut, const FCOLOR *pC)
{
    pOut->r = 1.0f - pC->r;
    pOut->g = 1.0f - pC->g;
    pOut->b = 1.0f - pC->b;
    pOut->a = pC->a;
    return pOut;
}
FINLINE FCOLOR* FColorAdd(FCOLOR *pOut, const FCOLOR *pC1, const FCOLOR *pC2)
{
    pOut->r = pC1->r + pC2->r;
    pOut->g = pC1->g + pC2->g;
    pOut->b = pC1->b + pC2->b;
    pOut->a = pC1->a + pC2->a;
    return pOut;
}
FINLINE FCOLOR* FColorSubtract(FCOLOR *pOut, const FCOLOR *pC1, const FCOLOR *pC2)
{
    pOut->r = pC1->r - pC2->r;
    pOut->g = pC1->g - pC2->g;
    pOut->b = pC1->b - pC2->b;
    pOut->a = pC1->a - pC2->a;
    return pOut;
}
FINLINE FCOLOR* FColorScale(FCOLOR *pOut, const FCOLOR *pC, float s)
{
    pOut->r = pC->r * s;
    pOut->g = pC->g * s;
    pOut->b = pC->b * s;
    pOut->a = pC->a * s;
    return pOut;
}
FINLINE FCOLOR* FColorModulate(FCOLOR *pOut, const FCOLOR *pC1, const FCOLOR *pC2)
{
    pOut->r = pC1->r * pC2->r;
    pOut->g = pC1->g * pC2->g;
    pOut->b = pC1->b * pC2->b;
    pOut->a = pC1->a * pC2->a;
    return pOut;
}
FINLINE FCOLOR* FColorLerp(FCOLOR *pOut, const FCOLOR *pC1, const FCOLOR *pC2, float s)
{
    s = FClamp(s, 0.0f, 1.0f);
    pOut->r = pC1->r + s * (pC2->r - pC1->r);
    pOut->g = pC1->g + s * (pC2->g - pC1->g);
    pOut->b = pC1->b + s * (pC2->b - pC1->b);
    pOut->a = pC1->a + s * (pC2->a - pC1->a);
    return pOut;
}
